#include "ransac.h"

using namespace cv;
int RandPointIndex_0;
int RandPointIndex_1;
// The generic RANSAC algorithm (http://en.wikipedia.org/wiki/RANSAC - see pseudocode)
bool CRansacLinear::getBestModel(LinearModel& model)
//bool CRansacLinear::run()
{
    int foundModel = 0;
    int iterations = 0;

    //mIterations = 1000;
    // Total iteration = 1000*990 = 990000
    
    while (iterations < mIterations) {

        vector<Point2d>  maybeInliers = getMaybeInliers();
        vector<Point2d>  consensusSet = maybeInliers;
        model= getModel(maybeInliers[0], maybeInliers[1]);
        //getModel(maybeInliers[0], maybeInliers[1]);
		mA = maybeInliers[0];
		mB = maybeInliers[1];

//	    printf("obs size=%d\n",mObservationSet.size());

		//cout<<"m["<<iterations<<"].slope="<<model.mSlope<<"m["<<iterations<<"].intercept="<<model.mIntercept<<endl;
		
        // In this loop, find the consensusSet.
        //cout<<"mObservationSet.size()="<<mObservationSet.size()<<endl;
        for(int i = 0; i <  mObservationSet.size(); i++) 
		{
			// except testing model itself, if do this, the error is always ZERO.
			if( (i==RandPointIndex_0) || (i==RandPointIndex_1)) continue;

			// If this ith sample is in the threshold, add it to consesnsuSet.
            if( fitsModel(mObservationSet[i], mA,mB)) 
			{
                    consensusSet.push_back(mObservationSet[i]);
            }


        }
		
        if(consensusSet.size() >= mRequiredInliers)
        //if(consensusSet.size() >= 6)
		{
            model            = getModel(consensusSet);
            double thisError  = getModelError(consensusSet, mA,mB);
			//cout<<"thisError="<<thisError<<" with m="<<model.mSlope<< " with b="<<model.mIntercept<<endl;
		    //cout<<"mBestError="<<mBestError<<endl;
            if(thisError < mBestError) 
			{
				for(int i=0;i<consensusSet.size();i++)
				{
						Point2d temp;
						temp.y=consensusSet[i].x;
						temp.x=consensusSet[i].y;
						mBestConsensusSet.push_back(temp);
				}
				//mBestConsensusSet = consensusSet;
				//mOutLiers= mObservationSet-mBestConsensusSet;
                mBestModel        = model;
                mBestError        = thisError;
                foundModel        = 1;
            }
        }
        iterations++;
    }

	#if 0
	if(foundModel)
	{
		cout<<"mBestConsensusSet.size="<<mBestConsensusSet.size()<<endl;
		cout<<"Best m = "<<mBestModel.mSlope<<" Best b = "<<mBestModel.mIntercept<<endl;
		cout<<"Best error ="<<mBestError<<endl;
		cout<<"Found model = "<<foundModel<<endl;
	}
	#endif

	//cout<<"mBestConsensusSet.size="<<mBestConsensusSet.size()<<endl;
	
	return foundModel;
}
RNG rng( 0xFFFFFFFF );
// Get two random points from observation-set
vector<Point2d> CRansacLinear::getMaybeInliers() const
{
    vector<Point2d> maybeInliers;
	
    int listSize = mObservationSet.size();
    RandPointIndex_0= rng.uniform(0,listSize);
	RandPointIndex_1 = rng.uniform(0,listSize);

    // Two points must have different index unless angle calculation fail.
	while(1)
	{
		if(RandPointIndex_0==RandPointIndex_1) 
			RandPointIndex_1 = rng.uniform(0,listSize);
		else break;
	}
	//cout<<"randPointIndex_0="<<RandPointIndex_0<<endl;
	//cout<<"randPointIndex_1="<<RandPointIndex_1<<endl;

    maybeInliers.push_back(mObservationSet[RandPointIndex_0]);
    maybeInliers.push_back(mObservationSet[RandPointIndex_1]);

    return maybeInliers;
}

// Get  linear equation parameters (slope and intercept) from two points
LinearModel CRansacLinear::getModel(const Point2d& p0, const Point2d& p1) const
{
    LinearModel model;

	//cout<<"p0.x="<<p0.x<<" p0.y="<<p0.y<<" p1.x="<<p1.x<<" p1.y="<<p1.y<<endl;

    //model.mSlope     = (p1.y-p0.y)/(p1.x-p0.x);
    model.mSlope     = atan2(p1.y-p0.y,p1.x-p0.x);

	model.mIntercept = p0.y - model.mSlope * p0.x;

    return model;
}

// Get linear equation parameters (slope and intercept) from observation data
// Use opencv to solve least-square equation (...i know, this can be done in many ways)
LinearModel CRansacLinear::getModel( vector<Point2d>& observation) 
{
    LinearModel model;

    unsigned int noDataPoints = observation.size();

	//Flip x,y from observation set

	for(int i=0;i<observation.size();i++)
	{
		float temp=0;
		temp=observation[i].x;
		observation[i].x=observation[i].y;
		observation[i].y=temp;
	}

    CvMat* ptA = cvCreateMat(noDataPoints, 2, CV_64FC1);
    CvMat* ptB = cvCreateMat(noDataPoints, 1, CV_64FC1);
    CvMat* ptX   = cvCreateMat(2,            1, CV_64FC1);

    for(unsigned int i=0; i < noDataPoints; i++) {
        cvmSet(ptA, i, 0, observation[i].x);
		cvmSet(ptA, i, 1, 1.0);
        cvmSet(ptB, i, 0, observation[i].y);
        //cvmSet(ptA, i, 1, 1.0);
        //cvmSet(ptB, i, 0, observation[i].y);
    }

    cvSolve(ptA, ptB, ptX, CV_SVD);

	model.mSlope     = cvmGet(ptX, 0, 0);
	//model.mSlope     = cvmGet(ptX, 0, 0);
	//float rho;
   model.mIntercept = cvmGet(ptX, 1, 0);
	//cout<<"model.mSlop="<<model.mSlope<<" model.mIntercept="<<model.mIntercept<<endl;
    //rho = model.mIntercept*sin(model.mSlope);

    cvReleaseMat(&ptA);
    cvReleaseMat(&ptB);
    cvReleaseMat(&ptX);

    return model;
}

// Check, whether point is a member of vector

// pList = our total observation points vector
// p = randomly selected two points from our observation data set.
// Point2d is consist of x,y in 2D cartecian coordinate.

bool CRansacLinear::isMember(const Point2d& p, const vector<Point2d>& pList) const
{
    bool isMember = false;

	//printf("pList.size()=%d\n",pList.size());


	cout<<"pList.size="<<pList.size()<<endl;
    for(int i = 0; i < pList.size(); i++) {
		//printf("p=%f pList[%d]=%f\n",p.x(),i,pList[i].x());
        if(p.x == pList[i].x) {
			printf("Member!\n");
			printf("px=%f py=%f pList[%d].x=%f pList[%d].y=%f\n",p.x,p.y,i,pList[i].x,i,pList[i].y);
			
            isMember = true;
        }
    }
    return isMember;
}

// Get distance between point and model
double CRansacLinear::getDistance(const Point2d& P, const Point2d& A, const Point2d& B) const
{
	 double distance = INT_MAX;

	 //This should return 10
	 #if 0 
     Point2d P,A,B;
     P.x = (double)170;
	 P.y = (double)120;
	 A.x = (double)160;
	 A.y = (double)0;
	 B.x = (double)160;
	 B.y = (double)240;
	 #endif
	 
	 //cout<<"P.x="<<P.x<<" P.y="<<P.y<<"A.x="<<A.x<<" A.y="<<A.y<<"B.x="<<B.x<<" B.y="<<B.y<<endl;

	 double normalLength= sqrt( (B.x-A.x)*(B.x-A.x)+(B.y-A.y)*(B.y-A.y));
	 double v = abs( (P.x-A.x)*(B.y-A.y) - (P.y-A.y)*(B.x-A.x));
	 distance = (double) (v/normalLength);
	 //cout<<"distance="<<distance<<endl;
    return distance;
}

// Check, whether distance of point from model is smaller than a given treshold
bool CRansacLinear::fitsModel(const Point2d& P,const Point2d& A,const Point2d& B) const
{
    bool fits = false;
    double distance = getDistance(P,A,B);

    if(distance < mTreshold) {
        fits = true;
    }

    return fits;
}

// Model error ~ maximum points fitting the model
double CRansacLinear::getModelError( const vector<Point2d>& pointList, const Point2d& A,const Point2d& B)
{
    double sum_dist_error =  INT_MAX;
	//int max_error = INT_MAX;

    vector<Point2d> consensusSet;
	vector<Point2d> outliers;
	vector<double> vaccum;
	
    for(int i = 0; i < pointList.size(); i++) 
	{
		//double distance_ = getDistance_old(pointList[i], model);
		double distance = getDistance(pointList[i], mA,mB);
		vaccum.push_back(distance);

        if(fitsModel(pointList[i],A,B)) 
		{
            consensusSet.push_back(pointList[i]);
        }
    }
	sum_dist_error = accumulate(vaccum.begin(),vaccum.end(),0.0f);
    return sum_dist_error;
}

#if 0
// Helper function to generate data-files (gnuplot...)
void CRansacLinear::dump(const vector<Point2d>& pointListF)
{
    QFile file;
    QString strData;

    static int file_index = 0;

    file.setFileName(QString("%1.dat").arg(file_index));
    file.open(QIODevice::WriteOnly);

    for(int i=0; i < pointListF.size(); i++) {
        double X = pointListF[i].x;
        double Y = pointListF[i].y;
        strData = QString("%1").arg(X, 0, 'f', 2) + " " + QString("%1").arg(Y, 0, 'f', 2)  + "\n";
        file.write(strData.toUtf8().data());
    }

    file_index++;
    file.close();
}
#endif

